'''
本程序采用模板匹配+实时更新。可以在目标发生简单变动后依然识别，但长时间运行有不稳定因素。
云台追踪中，也使用类似策略，但需要更详细纠错逻辑。
'''
import sensor, image, time,lcd
from os import mkdir
from button import BUTTON
from pyb import UART,Pin,Timer,Servo
from pid import PID

sensor.reset()
sensor.set_framesize(sensor.QQVGA)
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_vflip(True)
sensor.set_hmirror(True)
clock = time.clock()

pan_servo=Servo(3)    #左右控制PD12
tilt_servo=Servo(4)  #上下控制PD13
pan_start_angle=0.0
tilt_start_angle=-20.0 #朝上更容易看到人脸

pan_angle=pan_servo.angle(pan_start_angle)    #上下运动控制设置一个初始角度
tilt_angle=tilt_servo.angle(tilt_start_angle)   #左右舵运动控制，初始角度
pan_pid = PID(p=0.2, i=0, imax=90) #脱机运行或者禁用图像传输，使用这个PID
tilt_pid = PID(p=0.2, i=0, imax=90) #脱机运行或者禁用图像传输，使用这个PID
lcd.init(type=2,width=240,height=320)
lcd.set_direction(2)
button=BUTTON()  #声明按键，梦飞openmv只有一个按键，因此直接内部指定了按键

temp_width= 48
temp_height= 48
image_data = sensor.alloc_extra_fb(temp_width,temp_height, sensor.GRAYSCALE)

click_timer=time.ticks()          #开始计时
mode =0 #默认录入模式

def register_tamplate():
    while True:
        clock.tick()
        img = sensor.snapshot()
        if button.event():
            button.key_event=0
            ROI=(int(img.width()/2-15),int(img.height()/2-15),temp_width,temp_height)
            image_data.clear()
            image_data.draw_image(img,0,0,x_scale=1,y_scale=1,roi=ROI)#将roi画到模板画布上
            break
        img.draw_rectangle(int(img.width()/2-15),int(img.height()/2-15),temp_width,temp_height,thickness=2)
        lcd.display(img)

def traking_tamplate():
    #识别模式
    while True:
        img = sensor.snapshot()
        results = img.find_template(image_data, 0.70, step=2, search=image.SEARCH_EX)
        if results:
            print(results)
            #image_data.draw_image(img,0,0,roi=results)
            img.draw_rectangle(results,thickness=2,color=(255,0,0))
            #step1 计算目标距离图像中心的偏差
            pan_error=0
            flag_lost=0
            pan_error = img.width()/2-(results[2]/2+results[0])   #左右控制的偏差
            tilt_error =img.height()/2-(results[3]/2+results[1]) #上下控制的偏差
            pan_output=pan_pid.get_pid(pan_error,1)/2   #计算PID参数
            tilt_output=tilt_pid.get_pid(tilt_error,1)/2  #计算PID参数
            pan_angle=pan_servo.angle()+pan_output
            tilt_angle=tilt_servo.angle()-tilt_output
            if pan_angle>=60:   pan_angle=60
            if pan_angle<=-60:   pan_angle=-60
            if tilt_angle>=60:   tilt_angle=60
            if tilt_angle<=-60:   tilt_angle=-60
            pan_servo.angle(pan_angle)
            tilt_servo.angle(tilt_angle)
        lcd.display(img)
        if button.event():
            button.key_event=0
            break


while True:
    register_tamplate() #注册模式
    traking_tamplate()  #识别模式
    #长按退出
    if button.state():
        click_timer=time.ticks()          #开始计时
        while button.state():  pass       #等待按键抬起
        if time.ticks()-click_timer>2000: #按键时长超过2s
            sensor.dealloc_extra_fb()
            del image_data
            break                         #循环退出，回到主界面
    else :
        click_timer=time.ticks()#计时更新

